#include"kernel.h"
#include"sched.h"
#define MAX_TCB_NUM 10

//PendSV控制器基地址
#define NVIC_INT_CTRL (*((volatile uint32_t *)0xE000ED04))
//悬起PendSV
#define PEND_INT_SET  (1<<28)

uint tcb_total=0;
int new_tcb=-1;
int old_tcb=-1;

#pragma pack(push)
#pragma pack(1)
struct TCB{
    uint PSP;
    uint8 prio:3;                  //当前优先级    0~5(5为最高)
}tcb_list[MAX_TCB_NUM];         //进程描述符向量表
#pragma pack(pop)

uint task_stack[10][2*1024];    //进程堆栈
uint Core_stack[2*1024];        //内核堆栈

void kernel_init(){
    __ASM("PUSH {R0}\r\n"
        "MOVS R0,#0X0\r\n"
        "MSR PSP,R0\r\n"
        "POP {R0}\r\n"
        );
}

thread_t kernel_thread_create(void(*task)(void*),void*args){
    if(MAX_TCB_NUM<=tcb_total){
        return 0;
    }
    uint*sp=task_stack[tcb_total]+2*1024;
    *(--sp) = 0x01000000u;              // xPSR
	*(--sp) = (uint)task;               // PC
	*(--sp) = 0;                        // R14(LR)
	*(--sp) = 0x12121212u;              // R12
	*(--sp) = 0x03030303u;              // R3
	*(--sp) = 0x02020202u;              // R2
	*(--sp) = 0;                        // R1
	*(--sp) = (uint)args;               // R0

    *(--sp) = 0;                        // R11
	*(--sp) = 0;                        // R10
	*(--sp) = 0;                        // R9
	*(--sp) = 0;                        // R8
	*(--sp) = 0;                        // R7
	*(--sp) = 0;                        // R6
	*(--sp) = 0;                        // R5
	*(--sp) = 0;                        // R4
    tcb_list[tcb_total].PSP=(uint)sp;
    tcb_list[tcb_total].prio=5;
    ++tcb_total;
    return tcb_total;
}

int search_max_prio(){
    int ret=0;
    uint8 max=0;
    for(int i=0;i<tcb_total;++i){
        if(tcb_list[i].prio>max){
            max=tcb_list[i].prio;
            ret=i;
        }
    }
    return ret;
}

void tcb_switch(){                      //上下文切换 触发PendSV
    old_tcb=new_tcb;
    //new_tcb=search_max_prio();
    ++new_tcb;
    if(new_tcb==tcb_total){
        new_tcb=0;
    }
    --tcb_list[new_tcb].prio;
    if(tcb_list[new_tcb].prio==0){
        tcb_list[new_tcb].prio=5;
    }
    NVIC_INT_CTRL=PEND_INT_SET;
}

void kernel_start(){
    sched_init();
    tcb_switch();
    __ASM("CPSIE I\r\n");
}

void SysTick_Handler(){
    tcb_switch();
}